#include <SoftwareSerial.h>

// command heads
// 0x53 camera require
// 0x73 camera response
// 0x13 motion require
// 0x33 motion response

#define MOTOR_A_DIR 12
#define MOTOR_A_PWD 3
#define MOTOR_A_BRK 9

#define MOTOR_B_DIR 13
#define MOTOR_B_PWD 11
#define MOTOR_B_BRK 8

#define SERVO_5 5
#define SERVO_6 6

#define CAMERA_RX 2
#define CAMERA_TX 10

#define BUF_SIZE 64
#define CAMERA_BAUD_115200 0x0D, 0xA6

SoftwareSerial Camera(CAMERA_RX, CAMERA_TX);
byte cmd_buf[BUF_SIZE];

void motion(byte cmd_len, byte offset = 0);
void camera(byte cmd_len, byte offset = 0);

void setup()
{
  Serial.begin(115200);

  Camera.begin(38400);  // LS-Y201 default baud rate
  camera_baud_rate(CAMERA_BAUD_115200);
  Camera.end();
  Camera.begin(115200);
  
  // motion pin initialize

  Serial.println("setup end.");
}

void loop()
{
  byte cmd_len = 0;
  if (cmd_len = Serial.available())
  {
    for (byte i = 0; i < cmd_len; i++)
      cmd_buf[i] = Serial.read();

    switch (cmd_buf[0])
    {
      case 0x13: // motion require
        motion(cmd_len);
        break;
      case 0x53: // camera require
        camera(cmd_len);
        break;
      default: break;
    }
  }
}


void motion(byte cmd_len, byte offset/* = 0*/)
{
  // motor contorl
  // move
  // "w " : left  100% right  100%
  // "wa" : left    0% right  100%
  // "wd" : left  100% right    0%
  //
  // "s " : left -100% right -100%
  // "sa" : left    0% right -100%
  // "sd" : left -100% right    0%
  //
  // " a" : left -100% right  100%
  // " d" : left  100% right -100%
  // stop
  // cmd_len is 0 or
  // "  " : left    0% right    0%

  // servo contorl
  // "mh[-]?" : servo_h +/- (?)
  // "mv[-]?" : servo_v +/- (?)
}


void camera(byte cmd_len, byte offset/* = 0*/)
{
  for (byte i = 0; i < cmd_len; i++)
    Camera.write(cmd_buf[i]);

  byte recv_len = Camera.available();
  for (byte i = 0; i < recv_len; i++)
    Serial.write(Camera.read());
}

byte recv_camera(bool wait = true)
{
  // wait
  while (wait && Camera.available() == 0);
  // recv
  byte len = Camera.available();
  for (byte i = 0; i < len; i++)
    cmd_buf[i] = Camera.read();

  return len;
}

void camera_baud_rate(byte byte1, byte byte2)
{
  // send
  Camera.write((byte)0x56);
  Camera.write((byte)0x00);
  Camera.write((byte)0x24);
  Camera.write((byte)0x03);
  Camera.write((byte)0x01);
  Camera.write(byte1);
  Camera.write(byte2);
  // recv
  recv_camera();
}



